#!/usr/bin/env python
#!coding=utf-8
 
import rospy
import numpy as np
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2
 
cam0_path  = 'src/data/image/'    # 已经建立好的存储cam0 文件的目录

# camera_matrix = np.mat(((555.311715,0.000000,318.060117),(0.000000,555.208904,238.365198),(0.000000,0.000000,1.000000)))
# projection= np.mat(((554.847412,0.000000 ,317.410138 ,0.000000),(0.000000, 554.541992, 237.933926 ,0.000000),(0.000000 ,0.000000 ,1.000000 ,0.000000)))

camera_matrix = np.array([[-1.36986301e-03, -9.31879601e-06,  4.39455782e-01],
       [ 1.68125597e-19,  1.36054422e-03, -8.60544218e-01],
       [ 0.00000000e+00,  4.33680869e-19,  1.00000000e+00]])

class get_target_place(object):
    def __init__(self):
        self.camera_matrix = np.array([[-1.36986301e-03, -9.31879601e-06,  4.39455782e-01],
       [ 1.68125597e-19,  1.36054422e-03, -8.60544218e-01],
       [ 0.00000000e+00,  4.33680869e-19,  1.00000000e+00]])
    def prework(self,img):
        # img = cv2.imread("src/data/image/25.512000.jpg")
        hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HLS)
        sensitivity = 10
        lower_red_0 = np.array([0,100,100]) 
        upper_red_0 = np.array([sensitivity,255,255])
        lower_red_1 = np.array([180-sensitivity,100,100]) 
        upper_red_1 = np.array([180,255,255])

        mask_0 = cv2.inRange(hsv, lower_red_0, upper_red_0)
        mask_1 = cv2.inRange(hsv, lower_red_1, upper_red_1)
        mask = cv2.bitwise_or(mask_0, mask_1)
        res = cv2.bitwise_and(img, img, mask=mask)

        print(res)
        return res

    def get_place_from_picture(self,img):

        canny = cv2.Canny(img,50,150)
        #创建矩形结构单元
        kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(3, 3))
        #形态学处理,开运算

        dilated = cv2.dilate(canny,kernel)
        cnts,_= cv2.findContours(dilated.copy(),cv2.RETR_EXTERNAL,cv2 . CHAIN_APPROX_SIMPLE ) 
        ans = []
        for cnt in cnts:
            if cv2.contourArea(cnt)<100:
                continue
            M = cv2.moments( cnt)
            ## 矩形中心
            cX = int ( M [ "m10" ] / (M [ "m00" ]+0.01) )
            cY = int ( M [ "m01" ] / (M [ "m00" ]+0.01) ) 
            ans.append([cX,cY,1])
        return ans

    def get_real_place(self,place):
        box_in_world = np.dot(self.camera_matrix,place.T)
        return box_in_world

    def main(self,img):
        print("help")
        print(img)
        img = self.prework(img)
        print("new")
        place = self.get_place_from_picture(img)
        print("new")
        place_in_world= self.get_real_place(np.array(place))
        return place_in_world
 